#include "map.hpp"
#include "../utils/utils.hpp"
#include <cmath>

// Len is the length of each generated edge.
static const double Len = 1;

//double Map::Angles[] = {
//	0,
//	1*M_PI/4,
//	2*M_PI/4,
//	3*M_PI/4,
//	4*M_PI/4,
//	5*M_PI/4,
//	6*M_PI/4,
//	7*M_PI/4,
//};

double Map::Angles[] = {
	0,
	M_PI/2,
	M_PI,
	3*M_PI/2,
};

Map::Map() {
	// Node n;
	// n.id = 0;
	// n.pt = Pt(0, 0);
	// nodes.push_back(n);
}

Map::Map(const Map &o) : nodes(o.nodes), edges(o.edges) {
}

bool Map::operator==(const Map &o) const {
	return nodes == o.nodes && edges == o.edges;
}

bool Map::Node::operator==(const Node &o) const {
	if (id != o.id || entrydir != o.entrydir || pt != o.pt)
		return false;
	for (int j = 0; j < Map::Ndirs; j++) {
		if (e[j] != o.e[j])
			return false;
	}
	return true;
}

bool Map::Node::operator!=(const Node &o) const {
	return !(*this == o);
}

bool Map::Edge::operator==(const Edge &o) const {
	return id == o.id && n[0] == o.n[0] && n[1] == o.n[1];
}

bool Map::Edge::operator!=(const Edge &o) const {
	return !(*this == o);
}

bool Map::operator!=(const Map &o) const {
	return !(*this == o);
}

unsigned int Map::nnodes() const {
	unsigned int N = 0;
	for (auto n = nodes.begin(); n != nodes.end(); n++) {
		if (n->id >= 0)
			N++;
	}
	return N;
}

Map::Node &Map::addnode(int n) {
	assert (n >= 0);
	assert (nodes.size() <= (unsigned int) n || nodes[n].id < 0);
	if ((unsigned int) n >= nodes.size())
		nodes.resize(n+1);
	nodes[n].id = n;
	return nodes[n];
}

bool Map::hasnode(int n) const {
	return n >= 0 && (unsigned int) n < nodes.size() && nodes[n].id == n;
}

void Map::addedge(int id, int n, int m) {
	assert(n >= 0);
	assert(m >= 0);
	assert (nodes[n].id >= 0);
	assert (nodes[m].id >= 0);
	assert (id >= 0);
	assert (edges.size() <= (unsigned int) id || edges[id].id < 0);
	if ((unsigned int) id >= edges.size())
		edges.resize(id+1);
	edges[id].id = id;
	edges[id].n[0] = n;
	edges[id].n[1] = m;
	edges[id].line = LineSg(nodes[n].pt, nodes[m].pt);
}

bool Map::hasedge(int e) const {
	return e >= 0 && (unsigned int) e < edges.size() && edges[e].id == e;
}

unsigned int Map::grow(unsigned int N) {
	std::vector<int> g;
	for (auto n = nodes.begin(); n < nodes.end(); n++) {
		if (n->id < 0)
			continue;
		if (n->growable())
			g.push_back(n->id);
	}

	if (g.size() == 0)
		fprintf(stderr, "cannot grow\n");

	unsigned int cnt = 0;
	while (cnt < N && !g.empty()) {
		int i;
		// With some probability, use the most recently
		// added node.
		if (randgen.real() < 0.85)
			i = g.size()-1;
		else
			i = randgen.integer(0, g.size()-1);
		int n = g.at(i);
		g.at(i) = g.back();
		g.pop_back();

		int id = grownode(n);

		if (nodes[n].growable())
			g.push_back(n);

		if (id >= 0) {
			g.push_back(id);
			cnt++;
		}
	}

	return cnt;
}

int Map::grownode(int n) {
	std::vector<int> dirs;
	int entry = nodes[n].entrydir;
	// With some probability, if the entry direction is a
	// possibility, choose it.
	if (nodes[n].e[entry] == Node::MayEdge && randgen.real() < 0.95) {
		dirs.push_back(entry);
	} else {
		for (int d = 0; d < Ndirs; d++) {
			if (nodes[n].e[d] == Node::MayEdge)
				dirs.push_back(d);
		}
	}

	int d = dirs.at(randgen.integer(0, dirs.size()-1));
	Pt pt = nodes[n].pt;
	Pt end(pt.x+Len*cos(Angles[d]), pt.y+Len*sin(Angles[d]));
	LineSg line(pt, end);
	LineSg longline(pt, line.along(1+2*Epsilon));
	auto e = edges.begin();
	for (; e != edges.end(); e++) {
		if (e->id < 0 || e->n[0] == n || e->n[1] == n)
			continue;
		if (longline.hits(e->line))
			break;
	}
	if (e != edges.end()) {	// Hit something.
		nodes[n].e[d] = Node::NoEdge;
		return -1;
	}

	nodes.resize(nodes.size()+1);
	Node &m = nodes.back();
	m.id = nodes.size()-1;
	m.pt = end;
	m.entrydir = d;
	m.e[rev(d)] = edges.size();

	nodes[n].e[d] = edges.size();

	edges.resize(edges.size()+1);
	Edge &edge = edges.back();
	edge.id = edges.size();
	edge.n[0] = n;
	edge.n[1] = m.id;
	edge.line = longline;

	return m.id;
}

void Map::draw(std::string path) const {
	Pt min = Pt::inf(), max = Pt::neginf();
	for (auto n = nodes.begin(); n != nodes.end(); n++) {
		if (n->id < 0)
			continue;
		min.x = std::min(min.x, n->pt.x);
		max.x = std::max(max.x, n->pt.x);
		min.y = std::min(min.y, n->pt.y);
		max.y = std::max(max.y, n->pt.y);
	}
	unsigned int w = 500, h = 500;
	double xs = w / (max.x-min.x);
	double ys = h / (max.y-min.y);
	xs = std::min(xs, ys);
	ys = xs;

	Image img(w, h);

	for (auto e = edges.begin(); e != edges.end(); e++) {
		if (e->id < 0)
			continue;
		Pt p0 = e->line.p0.minus(min);
		p0.scale(xs, ys);
		Pt p1 = e->line.p1.minus(min);
		p1.scale(xs, ys);
		img.add(new Image::Line(LineSg(p0, p1), e->c, e->w));
	}

	for (auto n = nodes.begin(); n != nodes.end(); n++) {
		if (n->id < 0)
			continue;
		Pt pt = n->pt.minus(min);
		pt.scale(xs, ys);
		img.add(new Image::Pt(pt, n->c, n->r, 1));
	}

	img.saveeps(path.c_str(), false, 72/4 /* quarter inch margin */);
}

Graph Map::rescuegraph() const {
	Graph g(nodes.size());

	for (auto e = edges.begin(); e != edges.end(); e++) {
		if (e->id < 0)
			continue;
		g.addedge(e->n[0], e->n[1], Len);
	}

	return g;
}

bool Map::Node::growable() const {
	const int BrMax = 4;
	int br = 0;
	bool may = false;
	for (int i = 0; i < Ndirs; i++) {
		if (e[i] == MayEdge)
			may = true;
		// Limit the branching to BrMax
		if (e[i] >= 0 && ++br >= BrMax)
			return false;
	}
	return may;
}

std::vector<int> Map::neighbors(int n) const {
	std::vector<int> ns;
	for (int i = 0; i < Map::Ndirs; i++) {
		int e = nodes[n].e[i];
		if (e < 0)
			continue;

		assert(edges[e].id >= 0);
		assert(edges[e].n[0] >= 0);
		assert(edges[e].n[1] >= 0);

		assert(edges[e].n[0] == n || edges[e].n[1] == n);

		if (edges[e].n[0] != n)
			ns.push_back(edges[e].n[0]);
		if (edges[e].n[1] != n)
			ns.push_back(edges[e].n[1]);
	}
	return ns;
}

const Map::Edge *Map::edge(int n, int m) const {
	for (int i = 0; i < Map::Ndirs; i++) {
		int e = nodes[n].e[i];
		if (e < 0)
			continue;
		if (edges[e].n[0] == m)
			return &edges[e];
		if (edges[e].n[1] == m)
			return &edges[e];
	}
	return NULL;
}
